#ifndef VMC_LANE_FULL_MATCHING_UTILS
#define VMC_LANE_FULL_MATCHING_UTILS

#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
#include <vector>
#include <string>

#include <base/defines.h>
#include <base/camera.h>
#include <base/common.h>

namespace LFM
{
    class utils
    {
    public:
        static void saveAsOBJ(const std::string &filename, const std::vector<std::vector<Eigen::Vector3d>> &polylines);
        static ARSLane Fitting2d(const std::vector<Eigen::Vector2d>& geometry2d,
            cv::Mat K,
            cv::Mat D);

		static std::vector<ARSLane> FittingSpec(const DlLane& dlLane,
			cv::Mat K,
			cv::Mat D);

        static bool FindNearestPoint(const Eigen::Vector3d& ptHit,
            const hdmap_lane& lane, 
            Eigen::Vector3d& ptPrj, 
            double tolerance = 2.0);

        static Eigen::Vector3d WorldToCamera(const Eigen::Vector3d & xw3,
		    Eigen::Matrix3d wr,
		    Eigen::Vector3d wt
		    );

        static Eigen::Vector3d CameraToWorld(const Eigen::Vector3d& xc3, 
            Eigen::Matrix3d wr, 
            Eigen::Vector3d wt);

        static bool Recontruction( std::vector<ARSLane>& lane3Ds,
            const Camera &cam,
            const Mounting& mount,
            double scope = 50.0);

        static bool Translate3d_3d_LM(const std::vector<accordance_pair>& match_points, 
            Eigen::Matrix3d& R, 
            Eigen::Vector3d& T,
            double& error);

        static bool Translate3d_3d0(const std::vector<accordance_pair>& match_points, 
            Eigen::Matrix3d& R, 
            Eigen::Vector3d& T);

        static bool ComputeRT(const std::vector<ARSLane>& polylines, 
            const std::vector<hdmap_lane>& lanes,  
            Eigen::Matrix3d& R,
            Eigen::Vector3d& T, 
            double& rmse, 
            std::vector<accordance_pair>& match_pair_res,
            double tolerance);

        static void MappintDistort(const std::string &filename,
                                   cv::Mat K,
                                   cv::Mat D,
                                   cv::Mat &map11,
                                   cv::Mat &map12);

        static std::vector<hdmap_lane> LoadByMiddleLayer(const GeoPoint& gps_point,
            const std::vector<hdmap_lane>& hdmap_lanes,
            double interval = 3.0,
            double range = 100.0);

        static void CoordinateLocalize(const GeoPoint& gps_point, hdmap_lane& hdmap_lane);
        
        static double ComputeError(const std::vector<accordance_pair>& match_points,
            const Eigen::Matrix3d& R, 
            const Eigen::Vector3d& T);

        static std::vector<hdmap_lane> MergeHDlinks(
		    std::vector<hdmap_lane>& lanes,
            double tolerance = 0.5);

        static std::vector<ARSLane> MergeDLlinks(
		    std::vector<ARSLane>& lanes,
            double tolerance = 0.5);
	
        static double Score(const ARSLane& l1,
		    const ARSLane& l2,
		    const hdmap_lane& r1,
		    const hdmap_lane& r2);

        static bool Matching(const std::vector<ARSLane>& dlLanes,
            const std::vector<hdmap_lane>& hdLanes,
            std::vector<match_pair>& match_results);

        static bool CalVanishPt(const std::vector<ARSLane> &arsLanes,
            Eigen::Vector2d &vanish_pt);

        static void DumpSplines3D(const std::string &filename, 
            const std::vector<ARSLane> &lane3Ds,
            const std::vector<hdmap_lane> &hdlanes);
         

    };
} // namespace lfm

#endif